#include "Location.h"

#if defined(ESP8266)
#include <SoftwareSerial.h>
SoftwareSerial Serial8266(12, 13);
#else
#include <HardwareSerial.h>
#include <SPI.h>
#endif

L76XLocation::L76XLocation() {}

void L76XLocation::setBaudrate(unsigned long baudrate) {
#if defined(ESP8266)
  Serial8266.begin(baudrate, EspSoftwareSerial::SWSERIAL_8N1, 12, 13);
#elif defined(ESP32)
  /**
   * @brief 
   * 合宙ESP32-C3 使用的 IO0 和 IO1
   */
  Serial1.begin(baudrate, SERIAL_8N1, 0, 1);
#elif defined(BL616)
  /**
 * @brief 
 * AI-M61-32SU Serial1 使用的GPIO 为 IO24 和 I023 
 */
  Serial1.begin(baudrate, SERIAL_8N1);
#endif
}

void L76XLocation::sendCommand(char *data) {
  char Check = data[1], Check_char[3] = { 0 };
  uint8_t i = 0;
  writeByte('\r');
  writeByte('\n');

  for (i = 2; data[i] != '\0'; i++) {
    Check ^= data[i];  // Calculate the check value
  }

  Check_char[0] = Temp[Check / 16 % 16];
  Check_char[1] = Temp[Check % 16];
  Check_char[2] = '\0';

  writeString(data);
  writeByte('*');
  writeString(Check_char);
  writeByte('\r');
  writeByte('\n');
}

void L76XLocation::writeByte(char data) {
#if defined(ESP8266)
  Serial8266.write(data);
#else
  Serial1.write(data);
#endif
}

void L76XLocation::writeString(char *data) {
  for (uint16_t i = 0; data[i] != '\0'; i++) {
    writeByte(data[i]);
  }
}

char *L76XLocation::NMEA() {
  int i = 0;
  while (1) {
#if defined(ESP8266)
    if (Serial8266.available()) {
      char c = Serial8266.read();
#else
    if (Serial1.available()) {
      char c = Serial1.read();
#endif
      buff[i] = c;
      i++;
      if (i >= BUFFSIZE) {
        break;
      }
    }
  }
  return buff;
}

TinyGPSPlus L76XLocation::getGPS() {
  Serial.println(NMEA());
  for (int i = 0; i < BUFFSIZE; i++) {
    char c = buff[i];
    if (c != '\0') {
      gps.encode(c);
    }
  }
  return gps;
}


// 计算两点之间的大圆距离，单位：千米
double L76XLocation::haversine(double lat1, double lon1, double lat2, double lon2) {

  double dLat = radians(lat2 - lat1);
  double dLon = radians(lon2 - lon1);
  double a = sin(dLat / 2) * sin(dLat / 2) + cos(radians(lat1)) * cos(radians(lat2)) * sin(dLon / 2) * sin(dLon / 2);
  double c = 2 * atan2(sqrt(a), sqrt(1 - a));

  return EARTH_RADIUS * c;
}
